#include <Wire.h>
void setup()
{

  Serial.begin(9600);
  Wire.begin();
}



#define SLAVE_ADDRESS 2

void loop()
{
	//long first = millis();
	delay(200);

	getWinFormInput();
	setSlaveMouv();
	
	//setSlaveK();
	//setSlaveSensorCoefficient();

	getSlaveDataTest();
	reportWinforms();
	//Serial.println(millis() - first);
}

bool directionForward = true;
bool kpChanged = false;
bool kiChanged = false;
bool kdChanged = false;
bool sensorCoefficientChanged = false;

//stores commands from winform
uint8_t motor1DesiredRPM[3];
uint8_t motor2DesiredRPM[3];
uint8_t motor3DesiredRPM[3];
				 
uint8_t motor1_kp[3] = {'0', '1', '0'};
uint8_t motor1_ki[3] = {'0', '1', '0'};
uint8_t motor1_kd[3] = {'0', '0', '0'};

uint8_t motor2_kp[3]=  {'0', '1', '0'};
uint8_t motor2_ki[3]=  {'0', '1', '0'};
uint8_t motor2_kd[3]=  {'0', '0', '0'};
 
uint8_t motor3_kp[3]=  {'0', '1', '0'};
uint8_t motor3_ki[3]=  {'0', '1', '0'};
uint8_t motor3_kd[3]=  {'0', '0', '0'};

uint8_t motor1SensorCoeffient[3] = {'0', '1', '0'};
uint8_t motor2SensorCoeffient[3] = {'0', '1', '0'};
uint8_t motor3SensorCoeffient[3] = {'0', '1', '0'};

//used to format commands for slave
uint8_t mouvementCommand[12]; //e.g. F255F255F255 or B130B130B140
uint8_t kChangedCommand[12]; //e.g. P080P070P080  
uint8_t SpeedChangedCommand[12]; //e.g. P080P070P080  
uint8_t sensorCoefficientCommnad[12]; //e.g. E010E010E010


//stores status gotten from slave, to be returned to winform
uint8_t motor1RPM_i2cbyte = 5;
uint8_t motor1Current_i2cbyte  = 0;
uint8_t motor1Temp1_i2cbyte  = 0;
uint8_t motor1Temp2_i2cbyte  = 0;
uint8_t motor1Temp3_i2cbyte  = 0;
uint8_t motor1DesiredRPM_i2cbyte  = 0;
uint8_t motor1Kp_i2cbyte  = 0;
uint8_t motor1Ki_i2cbyte  = 0;
uint8_t motor1Kd_i2cbyte  = 0;
uint8_t motor1Coefficient_i2cbyte  = 0;
uint8_t motor1duty_i2cbyte  = 0;

uint8_t motor2RPM_i2cbyte  = 5;
uint8_t motor2Current_i2cbyte  = 0;
uint8_t motor2Temp1_i2cbyte  = 0;
uint8_t motor2Temp2_i2cbyte  = 0;
uint8_t motor2Temp3_i2cbyte  = 0;
uint8_t motor2DesiredRPM_i2cbyte  = 0;
uint8_t motor2Kp_i2cbyte  = 0;
uint8_t motor2Ki_i2cbyte  = 0;
uint8_t motor2Kd_i2cbyte  = 0;
uint8_t motor2Coefficient_i2cbyte  = 0;
uint8_t motor2duty_i2cbyte  = 0;

uint8_t motor3RPM_i2cbyte  = 5;
uint8_t motor3Current_i2cbyte  = 0;
uint8_t motor3Temp1_i2cbyte  = 0;
uint8_t motor3Temp2_i2cbyte  = 0;
uint8_t motor3Temp3_i2cbyte  = 0;
uint8_t motor3DesiredRPM_i2cbyte  = 0;
uint8_t motor3Kp_i2cbyte  = 0;
uint8_t motor3Ki_i2cbyte  = 0;
uint8_t motor3Kd_i2cbyte  = 0;
uint8_t motor3Coefficient_i2cbyte  = 0;
uint8_t motor3duty_i2cbyte  = 0;



void getWinFormInput()//get commands from  winform
{
	char command_3[3];
	if(Serial.available() > 3)
	{
		char input = Serial.read();
		char sub_input;
		
		switch(input)
		{
			case 'E': //coefficient to calibrate speed sensor
				sub_input = Serial.read();
				Serial.readBytes(command_3,3);
				switch(sub_input)
				{
				case '1':
					copy3IntArray(command_3,motor1SensorCoeffient);
					break;		 
				case '2':		 
					copy3IntArray(command_3,motor2SensorCoeffient);
					break;		 
				case '3':		 
					copy3IntArray(command_3,motor3SensorCoeffient);
					break;
				default:
					break;
				}
				break;
			case 'S':
				sub_input = Serial.read();
				Serial.readBytes(command_3,3);
				//Serial.println(command_3); //debug
				switch(sub_input)
				{
				case '1':
					copy3IntArray(command_3,motor1DesiredRPM);
					break;		 
				case '2':		 
					copy3IntArray(command_3,motor2DesiredRPM);
					break;		 
				case '3':		 
					copy3IntArray(command_3,motor3DesiredRPM);
					break;
				default:
					break;
				}
				break;
			case 'P':
				sub_input = Serial.read();
				Serial.readBytes(command_3,3);
				kpChanged = true;
				switch(sub_input)
				{
				case '1':
					
					copy3IntArray(command_3,motor1_kp);
					break;
				case '2':
					copy3IntArray(command_3,motor2_kp);
					break;
				case '3':
					copy3IntArray(command_3,motor3_kp);
					break;
				default:
					break;
				}
				
				break;
			case 'I':
				sub_input = Serial.read();
				Serial.readBytes(command_3,3);
				kiChanged = true;
				switch(sub_input)
				{
				case '1':
					copy3IntArray(command_3,motor1_ki);
					break;
				case '2':
					copy3IntArray(command_3,motor2_ki);
					break;
				case '3':
					copy3IntArray(command_3,motor3_ki);
					break;
				default:
					break;
				}
				
				break;
			case 'D':
				sub_input = Serial.read();
				Serial.readBytes(command_3,3);
				kdChanged = true;
				switch(sub_input)
				{
				case '1':
					copy3IntArray(command_3,motor1_kd);
					break;
				case '2':
					copy3IntArray(command_3,motor2_kd);
					break;
				case '3':
					copy3IntArray(command_3,motor3_kd);
					break;
				default:
					break;
				}
				
				break;
			case 't':
				Serial.readBytes(command_3,3);
				if(command_3[0] == '1' && command_3[1] == '0' && command_3[2] == '1')
				{
					if(directionForward == true)
					{
						directionForward = false;
						
					}
					else
					{
						directionForward = true;
					}
				}
				break;
			default:
				break;
		}
	}
} 

void setSlaveSensorCoefficient()
{
	if(sensorCoefficientChanged == true)
	{
		sensorCoefficientChanged = false;
		SpeedChangedCommand[0] = 'E';
		SpeedChangedCommand[4] = 'E';
		SpeedChangedCommand[8] = 'E';

		SpeedChangedCommand[1] = motor1SensorCoeffient[0];
		SpeedChangedCommand[2] = motor1SensorCoeffient[1];
		SpeedChangedCommand[3] = motor1SensorCoeffient[2];

		SpeedChangedCommand[5] = motor2SensorCoeffient[0];
		SpeedChangedCommand[6] = motor2SensorCoeffient[1];
		SpeedChangedCommand[7] = motor2SensorCoeffient[2];

		SpeedChangedCommand[9] =  motor3SensorCoeffient[0];
		SpeedChangedCommand[10] = motor3SensorCoeffient[1];
		SpeedChangedCommand[11] = motor3SensorCoeffient[2];

		Serial.write((char*)SpeedChangedCommand);
	}


}

void setSlaveK()
{
	if(kpChanged == true)
	{
		kpChanged = false;

		kChangedCommand[0] = 'P';
		kChangedCommand[4] = 'P';
		kChangedCommand[8] = 'P';

		kChangedCommand[1] = motor1_kp[0];
		kChangedCommand[2] = motor1_kp[1];
		kChangedCommand[3] = motor1_kp[2];

		kChangedCommand[5] = motor2_kp[0];
		kChangedCommand[6] = motor2_kp[1];
		kChangedCommand[7] = motor2_kp[2];

		kChangedCommand[9] =  motor3_kp[0];
		kChangedCommand[10] = motor3_kp[1];
		kChangedCommand[11] = motor3_kp[2];

		//Serial.write((char*)kChangedCommand);
		//send it

	}
	if(kiChanged == true)
	{
		kiChanged = false;


		kChangedCommand[0] = 'I';
		kChangedCommand[4] = 'I';
		kChangedCommand[8] = 'I';

		kChangedCommand[1] = motor1_ki[0];
		kChangedCommand[2] = motor1_ki[1];
		kChangedCommand[3] = motor1_ki[2];

		kChangedCommand[5] = motor2_ki[0];
		kChangedCommand[6] = motor2_ki[1];
		kChangedCommand[7] = motor2_ki[2];

		kChangedCommand[9] =  motor3_ki[0];
		kChangedCommand[10] = motor3_ki[1];
		kChangedCommand[11] = motor3_ki[2];

		//Serial.write((char*)kChangedCommand);

	}
	if(kdChanged == true)
	{
		kdChanged = false;

		kChangedCommand[0] = 'D';
		kChangedCommand[4] = 'D';
		kChangedCommand[8] = 'D';

		kChangedCommand[1] = motor1_kd[0];
		kChangedCommand[2] = motor1_kd[1];
		kChangedCommand[3] = motor1_kd[2];

		kChangedCommand[5] = motor2_kd[0];
		kChangedCommand[6] = motor2_kd[1];
		kChangedCommand[7] = motor2_kd[2];

		kChangedCommand[9] =  motor3_kd[0];
		kChangedCommand[10] = motor3_kd[1];
		kChangedCommand[11] = motor3_kd[2];
		
		//Serial.write((char*)kChangedCommand);
	}

}

void setSlaveMouv() //write commands to slave e.g. F255F255F255
{

	//Mouvement command
	if(directionForward == true)
	{
		mouvementCommand[0] = 'F';
		mouvementCommand[4] = 'F';
		mouvementCommand[8] = 'F';
	}
	else
	{
		mouvementCommand[0] = 'B';
		mouvementCommand[4] = 'B';
		mouvementCommand[8] = 'B';
	}

	mouvementCommand[1] = motor1DesiredRPM[0];
	mouvementCommand[2] = motor1DesiredRPM[1];
	mouvementCommand[3] = motor1DesiredRPM[2];

	mouvementCommand[5] = motor2DesiredRPM[0];
	mouvementCommand[6] = motor2DesiredRPM[1];
	mouvementCommand[7] = motor2DesiredRPM[2];

	mouvementCommand[9] =  motor3DesiredRPM[0];
	mouvementCommand[10] = motor3DesiredRPM[1];
	mouvementCommand[11] = motor3DesiredRPM[2];

	Wire.beginTransmission(SLAVE_ADDRESS);
	Wire.write(mouvementCommand, (size_t)12);
	Wire.endTransmission();

	//Serial.print("sending: "); Serial.print(((char*)mouvementCommand));Serial.println("|") //debug
}

void getSlaveData() //get status from slave
{
	Wire.requestFrom(2,15,true);
	motor1RPM_i2cbyte  = Wire.read();
	motor1Current_i2cbyte  = Wire.read();
	motor1Temp1_i2cbyte  = Wire.read(); 
	motor1Temp2_i2cbyte  = Wire.read(); 
	motor1Temp3_i2cbyte   = Wire.read();
	
	motor2RPM_i2cbyte   = Wire.read(); 
	motor2Current_i2cbyte   = Wire.read();
	motor2Temp1_i2cbyte   = Wire.read();
	motor2Temp2_i2cbyte   = Wire.read();
	motor2Temp3_i2cbyte   = Wire.read();
	
	motor3RPM_i2cbyte   = Wire.read(); 
	motor3Current_i2cbyte  = Wire.read();
	motor3Temp1_i2cbyte   = Wire.read();
	motor3Temp2_i2cbyte   = Wire.read();
	motor3Temp3_i2cbyte   = Wire.read();		

	
}

void getSlaveDataTest() //get status from slave
{
	if(Wire.requestFrom(2,9,true) == 9)
	{
		motor1RPM_i2cbyte  = (uint8_t) Wire.read();
		motor1DesiredRPM_i2cbyte  = Wire.read();
		motor1duty_i2cbyte = Wire.read();
	
		motor2RPM_i2cbyte  = Wire.read();
		motor2DesiredRPM_i2cbyte  = Wire.read();
		motor2duty_i2cbyte = Wire.read();

		motor3RPM_i2cbyte  = Wire.read();
		motor3DesiredRPM_i2cbyte  = Wire.read();
		motor3duty_i2cbyte = Wire.read();

	
		/*Serial.println(motor1DesiredRPM_i2cbyte);
		Serial.println("");*/
	}
	
}

void reportWinforms() //transfer the data acquired from slave to the winform
{
	 
	String report = "R1";
	//dtostrf(motors[0]->RPM,3,2,TempChar);
	report += String((unsigned int)motor1RPM_i2cbyte ); 
	report += "|";

	report += "R2";
	//dtostrf(motors[1]->RPM,3,2,TempChar);
	report += String((unsigned int)motor2RPM_i2cbyte ); 
	report += "|";

	report += "R3";
	//dtostrf(motors[2]->RPM,3,2,TempChar);
	report += String((unsigned int)motor3RPM_i2cbyte ); 
	report += "|";

	Serial.print(report);
  
	report = "";

	report += "S1";
	report += String((unsigned int)motor1DesiredRPM_i2cbyte ); 
	report += "|";

	report += "S2";
	//dtostrf(motors[1]->RPM,3,2,TempChar);
	report += String((unsigned int)motor2DesiredRPM_i2cbyte ); 
	report += "|";

	report += "S3";
	//dtostrf(motors[2]->RPM,3,2,TempChar);
	report += String((unsigned int)motor3DesiredRPM_i2cbyte ); 
	report += "|";

	Serial.print(report);
	
	report = "";

	report += "D1";
	report += String((unsigned int)motor1duty_i2cbyte ); 
	report += "|";

	report += "D2";
	//dtostrf(motors[1]->RPM,3,2,TempChar);
	report += String((unsigned int)motor2duty_i2cbyte  ); 
	report += "|";

	report += "D3";
	//dtostrf(motors[2]->RPM,3,2,TempChar);
	report += String((unsigned int)motor3duty_i2cbyte  ); 
	report += "|";

	Serial.print(report);

	//report = "";

	//report += "p1";
	//report += String((unsigned int)motor1Kp_i2cbyte ); 
	//report += "|";

	//report += "p2";
	////dtostrf(motors[1]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor2Kp_i2cbyte ); 
	//report += "|";

	//report += "p3";
	////dtostrf(motors[2]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor3Kp_i2cbyte ); 
	//report += "|";

	//Serial.print(report);

	//report = "";

	//report += "i1";
	//report += String((unsigned int)motor1Ki_i2cbyte ); 
	//report += "|";

	//report += "i2";
	////dtostrf(motors[1]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor2Ki_i2cbyte ); 
	//report += "|";

	//report += "i3";
	////dtostrf(motors[2]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor3Ki_i2cbyte ); 
	//report += "|";

	//Serial.print(report);

	//report = "";

	//report += "d1";
	//report += String((unsigned int)motor1Kd_i2cbyte ); 
	//report += "|";

	//report += "d2";
	////dtostrf(motors[1]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor2Kd_i2cbyte ); 
	//report += "|";

	//report += "d3";
	////dtostrf(motors[2]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor3Kd_i2cbyte ); 
	//report += "|";

	//Serial.print(report);


	//report = "";

	//report += "C1";
	//report += String((unsigned int)motor1Current_i2cbyte ); 
	//report += "|";

	//report += "C2";
	////dtostrf(motors[1]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor2Current_i2cbyte ); 
	//report += "|";

	//report += "C3";
	////dtostrf(motors[2]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor3Current_i2cbyte ); 
	//report += "|";

	//Serial.print(report);

	//report = "";

	//report += "T1";
	//report += String((unsigned int)motor1Temp1_i2cbyte ); 
	//report += "|";

	//report += "T2";
	////dtostrf(motors[1]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor2Temp1_i2cbyte ); 
	//report += "|";

	//report += "T3";
	////dtostrf(motors[2]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor3Temp1_i2cbyte ); 
	//report += "|";

	//Serial.print(report);


	//report = "";

	//report += "E1";
	//report += String((unsigned int)motor1Coefficient_i2cbyte ); 
	//report += "|";

	//report += "E2";
	////dtostrf(motors[1]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor2Coefficient_i2cbyte ); 
	//report += "|";

	//report += "E3";
	////dtostrf(motors[2]->RPM,3,2,TempChar);
	//report += String((unsigned int)motor3Coefficient_i2cbyte ); 
	//report += "|";

	//Serial.print(report);
}


void copy3IntArray(char original[], uint8_t target[])
{
	for(int i = 0 ; i < 3; i++)
	{
		target[i] = (int)original[i];
	}
}

